%% 用于测试自己写的函数而创建的文件

function test()
%% 绘制椭圆的函数
%     function ellipse(x0,y0,a,b)   %画椭圆函数
%         theta = 0:0.01:2*pi;
%         xp = x0 + a * cos(theta);
%         yp = y0 + b * sin(theta);
%         plot(xp, yp, '.r');
%     end
% 
%     ellipse(5,5,5,5)
%     axis equal
    
%% 测试getSample函数
%     function node = getSample(ax,bx,ay,by)   %1
%         % ax是x的搜索范围的下界
%         % bx是x的搜索范围的上界
%         % ay是y的搜索范围的下界
%         % by是y的搜索范围的上界
%         node = [0;0];
%         node(1) = (bx-ax) * rand(1) + ax;
%         node(2) = (by-ay) * rand(1) + ay;
%     end
%     
%     getSample(-2,10,0,12)
    
%% 测试isObstacleFree函数，障碍物是椭圆
    function free = isObstacleFree(node_free)
        % if it is obstacle-free, return 1.
        % otherwise, return 0
        free = 1;
        nx = node_free(1);
        ny = node_free(2);
        ha = (nx-5)^2 / 5^2 + (ny-5)^2 / 5^2;    
        if (ha <= 1)
        	free = 0;
        end 
    end

%     isObstacleFree([8.5361, 8.5361])
    
%% 验证采样函数getSample
    function node = getSample()   %1
%         x = 0;
%         y = 0;
%         a = 0;
%         b = 200;
%         node = [x;y];
        ax = -2;  % ax是x的搜索范围的下界
        bx = 10;  % bx是x的搜索范围的上界
        ay = 0;   % ay是y的搜索范围的下界
        by = 12;  % by是y的搜索范围的上界
% %         node = [0, 0];
%         node(1) = (bx-ax) * rand(1) + ax;
%         node(2) = (by-ay) * rand(1) + ay;
        free = 0;
        while ~free
            node(1) = (bx-ax) * rand(1) + ax;
            node(2) = (by-ay) * rand(1) + ay;
            if isObstacleFree(node)
                free = 1;
            end
        end
    end

    getSample()
end